<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
  
<script type="text/x-mathjax-config">
MathJax.Hub.Config({
  extensions: ["tex2jax.js"],
  jax: ["input/TeX","output/NativeMML"],
});
</script>
<script type="text/javascript" async="async" src="http://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML" type="text/javascript"></script>

<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.9.2"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>kalman-cpp: kalman-cpp</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<<<<<<< HEAD
<script type="text/x-mathjax-config">
  MathJax.Hub.Config({
    extensions: ["tex2jax.js"],
    jax: ["input/TeX","output/HTML-CSS"],
});
</script>
<script type="text/javascript" async="async" src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML" type="text/javascript/MathJax.js"></script>
=======
>>>>>>> c467bb8b4126460c1e18ba28ee4392f49fdc6552
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">kalman-cpp
   </div>
   <div id="projectbrief">Implementation of Kalman Filter in C++</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.9.2 -->
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(function() {
  initMenu('',false,false,'search.php','Search');
});
/* @license-end */</script>
<div id="main-nav"></div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(document).ready(function(){initNavTree('index.html',''); initResizable(); });
/* @license-end */
</script>
<div id="doc-content">
<div class="PageDoc"><div class="header">
  <div class="headertitle">
<div class="title">kalman-cpp </div>  </div>
</div><!--header-->
<div class="contents">
<div class="textblock"><h1><a class="anchor" id="linear-kalman"></a>
Kalman Filter for Linear Systems</h1>
<h2><a class="anchor" id="definition_kf"></a>
Definition</h2>
<p>A linear system is described as follows. </p><p class="formulaDsp">
\[x_k = Ax_{k-1} + Bu_{k-1} + v_{k-1}\]
</p>
 <p class="formulaDsp">
\[z_k = Hx_k + w_k\]
</p>
<p> where:<br  />
 \(v\) is the process noise (Gaussian with covariance Q)<br  />
 <br  />
 \(w\) is the measurement noise (Gaussian with covariance R)<br  />
 \(A\) is the system matrix<br  />
 \(B\) is the input matrix<br  />
 \(H\) is the output matrix<br  />
 \(x\) is the state vector<br  />
 \(z\) is the output vector<br  />
 \(u\) is the input vector<br  />
<br  />
The noise covariance matrices must follow these conditions:<br  />
 </p><p class="formulaDsp">
\[Q = Q^T\]
</p>
 <p class="formulaDsp">
\[R = R^T\]
</p>
<h2><a class="anchor" id="example_kf"></a>
Examples</h2>
<<<<<<< HEAD
<p>An example is provided in <a class="el" href="main1_8cpp.html" title="Example for the Kalman filter.">main1.cpp</a> solves <a href="http://www.mathworks.com/matlabcentral/fileexchange/18465-learning-the-kalman-filter-in-simulink-v2-1/content/html/runkalmanfilter.html">the car voltmeter problem</a> using this library. The system model for this problem is governed by: <br  />
=======
<p>An example provided in <a class="el" href="main1_8cpp.html" title="Example for the Kalman filter.">main1.cpp</a> solves<a href="http://www.
  works.com/matlabcentral/fileexchange/18465-learning-the-kalman-filter-in-simulink-v2-1/content/html/runkalmanfilter.html">the car voltmeter problem</a> using this library. The system model for this problem is governed by: <br  />
>>>>>>> c467bb8b4126460c1e18ba28ee4392f49fdc6552
 </p><p class="formulaDsp">
\[x_k = 12 + v_{k-1}\]
</p>
 <p class="formulaDsp">
\[z_k = x_k + w_ḳ\]
</p>
<p> <br  />
Let us extract the model parameters from the equation above: \(A = 0\), \(B = 1\), \(H = 1\), \(u = 12\), \(x_0 = 12\), \(Q=4\), and \(R=4\). Now that we know all the parameter values, we can then set up the following piece of code: <br  />
</p><div class="fragment"><div class="line">mat A(1,1), B(1,1), H(1,1), Q(1,1), R(1,1);</div>
<div class="line">colvec u(1);  </div>
<div class="line"> </div>
<div class="line">A &lt;&lt; 0;</div>
<div class="line">B &lt;&lt; 1;</div>
<div class="line">Q &lt;&lt; 4;</div>
<div class="line">H &lt;&lt; 1;</div>
<div class="line">R &lt;&lt; 4;</div>
<div class="line">u &lt;&lt; 12.0;</div>
</div><!-- fragment --><p>The first and second line of the code above are used to create all the necessary matrices with correct dimensions. In the following lines, we apply the correct value for each matrix.</p>
<p>Our next step is to create an instance of class class and initialize it with the previously created \(A\), \(B\), \(Q\), \(H\), and \(R\) matrices. <br  />
</p><div class="fragment"><div class="line"><a class="code" href="class_k_f.html">KF</a> kalman;</div>
<div class="line">kalman.<a class="code" href="class_k_f.html#a890367dd9a8888a2ea8bda296b6128dd">InitSystem</a>(A, B, H, Q, R);</div>
<div class="ttc" id="aclass_k_f_html"><div class="ttname"><a href="class_k_f.html">KF</a></div><div class="ttdoc">Kalman filter implementation, for a linear system.</div><div class="ttdef"><b>Definition:</b> kf.h:37</div></div>
<div class="ttc" id="aclass_k_f_html_a890367dd9a8888a2ea8bda296b6128dd"><div class="ttname"><a href="class_k_f.html#a890367dd9a8888a2ea8bda296b6128dd">KF::InitSystem</a></div><div class="ttdeci">void InitSystem(const mat &amp;A, const mat &amp;B, const mat &amp;H, const mat &amp;Q, const mat &amp;R)</div><div class="ttdoc">Define the system.</div><div class="ttdef"><b>Definition:</b> kf.cpp:20</div></div>
</div><!-- fragment --><p>And or our last step, we run the Kalman procedure repetitively as shown in the code below. <br  />
</p><div class="fragment"><div class="line"><span class="keywordflow">for</span> (<span class="keywordtype">int</span> k = 0; k &lt; 100; k++) {</div>
<div class="line">  kalman.<a class="code" href="class_k_f.html#acc63e676e77b8a6b1ccbb88dec545d99">Kalmanf</a>(u);</div>
<div class="line">  </div>
<div class="line">  <span class="comment">// The rest of the code </span></div>
<div class="line">}</div>
<div class="ttc" id="aclass_k_f_html_acc63e676e77b8a6b1ccbb88dec545d99"><div class="ttname"><a href="class_k_f.html#acc63e676e77b8a6b1ccbb88dec545d99">KF::Kalmanf</a></div><div class="ttdeci">void Kalmanf(const colvec &amp;u)</div><div class="ttdoc">Do Kalman filter iteration step-by-step while simulating the system. Simulating the system is done to...</div><div class="ttdef"><b>Definition:</b> kf.cpp:71</div></div>
</div><!-- fragment --><p> <br  />
</p><div class="image">
<img src="ex1.png" alt=""/>
</div>
<h1><a class="anchor" id="non-linear-kalman"></a>
Extended Kalman Filter (EKF) for nonlinear systems</h1>
<h2><a class="anchor" id="definition_ekf"></a>
Definition</h2>
<p>A non-linear system is described by: </p><p class="formulaDsp">
\[x_k = f(x_{k-1}, u_{k-1}) + v_{k-1}\]
</p>
 <p class="formulaDsp">
\[z_k = h(x_k) + w_k\]
</p>
<p> where:<br  />
 \(f\) is the dynamic model of the system<br  />
 \(h\) is the measurement model of the system<br  />
 \(v\) is the process noise (Gaussian with covariance Q)<br  />
 <br  />
 \(w\) is the measurement noise (Gaussian with covariance R)<br  />
 \(x\) is the state vector<br  />
 \(z\) is the output vector<br  />
 \(u\) is the input vector<br  />
<br  />
The noise covariance matrices must follow these conditions:<br  />
 </p><p class="formulaDsp">
\[Q = Q^T\]
</p>
 <p class="formulaDsp">
\[R = R^T\]
</p>
<h2><a class="anchor" id="example_ekf"></a>
Example</h2>
<p>Here, let us take <a class="el" href="main4_8cpp.html" title="Example for the extended Kalman filter.">main4.cpp</a> as an example. This content of this file is adapted from this <br  />
 <a href="http://ch.mathworks.com/matlabcentral/fileexchange/38302-kalman-filter-package/content//Kalman%20Filter%20Package/Examples/ExtendedKalmanFilterDemo.m">MATLAB Central page.</a> <br  />
<br  />
Let us define a nonlinear system as follows. <br  />
<br  />
 </p><p class="formulaDsp">
\[ f = \begin{bmatrix} \sin(x_2(k-1))(k-1) \\ x_2(k-1) \end{bmatrix} \]
</p>
 <p class="formulaDsp">
\[ h = \begin{bmatrix} x_1(k) \\ x_2(k) \end{bmatrix} \]
</p>
 <p class="formulaDsp">
\[ x_0 = \begin{bmatrix} 0 \\ \frac{1 \pi}{500} \end{bmatrix}\]
</p>
<p> <br  />
A nonlinear function does not have a strict tempate as in a linear function. Therefore, we need to let the user to derive the <a class="el" href="class_e_k_f.html" title="Implemetation of the extended Kalman filter. This class needs to be derived.">EKF</a> class for flexible implementation of a nonlinear model for both the process and the the measurement. <br  />
</p><div class="fragment"><div class="line"><span class="keyword">class </span>MyEKF: <span class="keyword">public</span> <a class="code" href="class_e_k_f.html">EKF</a></div>
<div class="line">{</div>
<div class="line"><span class="keyword">public</span>:  </div>
<div class="line">  <span class="keyword">virtual</span> colvec <a class="code" href="class_e_k_f.html#a6c084d493687d9ad0cf55d8c97c22633">f</a>(<span class="keyword">const</span> colvec&amp; x, <span class="keyword">const</span> colvec&amp; u) {</div>
<div class="line">    colvec xk(nOutputs_);</div>
<div class="line">    xk(0) = sin(x(1) * u(0));</div>
<div class="line">    xk(1) = x(1);</div>
<div class="line">    <span class="keywordflow">return</span> xk;</div>
<div class="line">  }</div>
<div class="line">  </div>
<div class="line">  <span class="keyword">virtual</span> colvec <a class="code" href="class_e_k_f.html#acb8638938ff97e5839e02f24860243e5">h</a>(<span class="keyword">const</span> colvec&amp; x) {</div>
<div class="line">    colvec zk(nOutputs_);</div>
<div class="line">    zk(0) = x(0);</div>
<div class="line">    zk(1) = x(1);</div>
<div class="line">    <span class="keywordflow">return</span> zk;</div>
<div class="line">  }</div>
<div class="line">};</div>
<div class="ttc" id="aclass_e_k_f_html"><div class="ttname"><a href="class_e_k_f.html">EKF</a></div><div class="ttdoc">Implemetation of the extended Kalman filter. This class needs to be derived.</div><div class="ttdef"><b>Definition:</b> ekf.h:37</div></div>
<div class="ttc" id="aclass_e_k_f_html_a6c084d493687d9ad0cf55d8c97c22633"><div class="ttname"><a href="class_e_k_f.html#a6c084d493687d9ad0cf55d8c97c22633">EKF::f</a></div><div class="ttdeci">virtual colvec f(const colvec &amp;x, const colvec &amp;u)</div><div class="ttdoc">Define model of your system.</div><div class="ttdef"><b>Definition:</b> ekf.cpp:60</div></div>
<div class="ttc" id="aclass_e_k_f_html_acb8638938ff97e5839e02f24860243e5"><div class="ttname"><a href="class_e_k_f.html#acb8638938ff97e5839e02f24860243e5">EKF::h</a></div><div class="ttdeci">virtual colvec h(const colvec &amp;x)</div><div class="ttdoc">Define the output model of your system.</div><div class="ttdef"><b>Definition:</b> ekf.cpp:67</div></div>
</div><!-- fragment --><p>The next steps below are very similiar to the previous example in the linear Kalman filter section. First, we start by creating all necessary matrices and set their values accordingly. Afterward, we continue with initializing. the covariance matrices as well as the state vector. In the final step, we run the <a class="el" href="class_e_k_f.html" title="Implemetation of the extended Kalman filter. This class needs to be derived.">EKF</a> procedurerepetitively while logging necessary data.</p>
<p><br  />
<br  />
</p><div class="fragment"><div class="line"><span class="keywordtype">int</span> <a class="code" href="main1_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a>(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>** argv)</div>
<div class="line">{</div>
<div class="line">   <span class="comment">//</span></div>
<div class="line">   <span class="comment">// Log the result into a tab delimitted file, later we can open </span></div>
<div class="line">   <span class="comment">// it with Matlab. Use: plot_data4.m to plot the results.</span></div>
<div class="line">   <span class="comment">//</span></div>
<div class="line">  ofstream log_file;</div>
<div class="line">  log_file.open(<span class="stringliteral">&quot;log_file4.txt&quot;</span>);</div>
<div class="line">  </div>
<div class="line">  <span class="keywordtype">int</span> n_states = 2;</div>
<div class="line">  <span class="keywordtype">int</span> n_outputs = 2;</div>
<div class="line">  mat Q(2, 2);</div>
<div class="line">  mat R(2, 2);</div>
<div class="line">  </div>
<div class="line">  Q &lt;&lt; 0.001 &lt;&lt; 0    &lt;&lt; endr</div>
<div class="line">    &lt;&lt; 0     &lt;&lt; 0    &lt;&lt; endr;</div>
<div class="line">    </div>
<div class="line">  R &lt;&lt; 0.1   &lt;&lt; 0    &lt;&lt; endr</div>
<div class="line">    &lt;&lt;   0   &lt;&lt; 0.01 &lt;&lt; endr;</div>
<div class="line">  </div>
<div class="line">  colvec x0(2);</div>
<div class="line">  x0 &lt;&lt; 0 &lt;&lt; 1 * M_PI / 500;</div>
<div class="line">  </div>
<div class="line">  colvec u(1);</div>
<div class="line">   </div>
<div class="line">  MyEKF myekf;</div>
<div class="line">  myekf.InitSystem(n_states, n_outputs, Q, R);</div>
<div class="line">  myekf.InitSystemState(x0);</div>
<div class="line">  </div>
<div class="line">  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> k = 0; k &lt; 1000; k ++) {</div>
<div class="line">    u(0) = k;</div>
<div class="line">    myekf.EKalmanf(u);</div>
<div class="line">    </div>
<div class="line">    colvec *x = myekf.GetCurrentState();</div>
<div class="line">    colvec *x_m = myekf.GetCurrentEstimatedState();</div>
<div class="line">    colvec *z = myekf.GetCurrentOutput();</div>
<div class="line">    </div>
<div class="line">    log_file &lt;&lt; k </div>
<div class="line">             &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; z-&gt;at(0,0) &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; x-&gt;at(0,0) &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; x_m-&gt;at(0,0)</div>
<div class="line">             &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; z-&gt;at(1,0) &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; x-&gt;at(1,0) &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; x_m-&gt;at(1,0)</div>
<div class="line">             &lt;&lt; <span class="charliteral">&#39;\n&#39;</span>; </div>
<div class="line">  }</div>
<div class="line">  </div>
<div class="line">  log_file.close();</div>
<div class="line">  </div>
<div class="line">  <span class="keywordflow">return</span> 0;</div>
<div class="line">}</div>
<div class="ttc" id="amain1_8cpp_html_a3c04138a5bfe5d72780bb7e82a18e627"><div class="ttname"><a href="main1_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a></div><div class="ttdeci">int main(int argc, char **argv)</div><div class="ttdef"><b>Definition:</b> main1.cpp:19</div></div>
</div><!-- fragment --><div class="image">
<img src="ex4.png" alt=""/>
</div>
<h1><a class="anchor" id="unscented-kalman"></a>
Unscented Kalman Filter (UKF) for a nonlinear system</h1>
<p>Another type of Kalman Filter for a nonlinear system is the Unscented Kalman Filter. It addresses the accuracy problem which arises during linearization process of an Extended Kalman filter when Jacobian is used. The Unscented Kalman Filter is implemented in class <a class="el" href="class_u_k_f.html" title="Implemetation of the Unscented Kalman filter. This class needs to be derived.">UKF</a> and the steps for using the provided <a class="el" href="class_u_k_f.html" title="Implemetation of the Unscented Kalman filter. This class needs to be derived.">UKF</a> class is exactly similiar as in using the <a class="el" href="class_e_k_f.html" title="Implemetation of the extended Kalman filter. This class needs to be derived.">EKF</a> class. <br  />
<br  />
The implementation of the <a class="el" href="class_u_k_f.html" title="Implemetation of the Unscented Kalman filter. This class needs to be derived.">UKF</a> in this C++ library is based on the following paper: <br  />
<br  />
Wan, E. A., &amp; Van Der Merwe, R. (2006). The unscented Kalman filter for nonlinear estimation. Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373), 31(2), 153–158. <br  />
<br  />
The paper above can be downloaded from <a href="https://doi.org/10.1109/ASSPCC.2000.882463">here</a>.</p>
<h2><a class="anchor" id="example_ukf"></a>
Example</h2>
<p>Here, we take <a class="el" href="main10_8cpp.html" title="An example for an Unscented Kalman filter (UKF).">main10.cpp</a> as an example. This example is adapted from this <br  />
 <br  />
This example is taken from<a href="https://www.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter">here</a>. The nonlinear system is described as follows. <br  />
 </p><p class="formulaDsp">
\[ f = \begin{bmatrix} x_2(k) \\ x_3(k) \\ 0.005 \, x_1(k) \bigg(x_2(k) + x_3(k) \bigg) \end{bmatrix} \]
</p>
 <p class="formulaDsp">
\[ h = \begin{bmatrix} x_1(k) \end{bmatrix} \]
</p>
 <p class="formulaDsp">
\[ x_0 = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}\]
</p>
<p> <br  />
The following lines of codes show how to use the <a class="el" href="class_u_k_f.html" title="Implemetation of the Unscented Kalman filter. This class needs to be derived.">UKF</a> class. As mentioned before, we need to first derive the <a class="el" href="class_u_k_f.html" title="Implemetation of the Unscented Kalman filter. This class needs to be derived.">UKF</a> class and define the virtual functions f (process function) and h (output function). <br  />
<br  />
</p><div class="fragment"><div class="line"><span class="keyword">class </span>MyUKF : <span class="keyword">public</span> <a class="code" href="class_u_k_f.html">UKF</a></div>
<div class="line">{</div>
<div class="line"><span class="keyword">public</span>:</div>
<div class="line">    <span class="keyword">virtual</span> colvec <a class="code" href="class_u_k_f.html#a3121b2c76798ca333cd2b292daa1a32c">f</a>(<span class="keyword">const</span> colvec&amp; x, <span class="keyword">const</span> colvec&amp; u) {</div>
<div class="line">        colvec xk(nStates_);</div>
<div class="line">        xk.at(0) = x(1);</div>
<div class="line">        xk.at(1) = x(2);</div>
<div class="line">        xk.at(2) = 0.05*x(0)*(x(1)+x(2));</div>
<div class="line">        <span class="keywordflow">return</span> xk;</div>
<div class="line">    }</div>
<div class="line"> </div>
<div class="line">    <span class="keyword">virtual</span> colvec <a class="code" href="class_u_k_f.html#ae0be58f48525b8a77bfbfe0091615519">h</a>(<span class="keyword">const</span> colvec&amp; x) {</div>
<div class="line">        colvec zk(nOutputs_);</div>
<div class="line">        zk(0) = x(0);</div>
<div class="line">        <span class="keywordflow">return</span> zk;</div>
<div class="line">    }</div>
<div class="line">};</div>
<div class="line"> </div>
<div class="line"> </div>
<div class="line"><span class="keywordtype">int</span> <a class="code" href="main1_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a>(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>** argv)</div>
<div class="line">{</div>
<div class="line">    mat Q(3, 3);</div>
<div class="line">    mat R(1, 1);</div>
<div class="line"> </div>
<div class="line"> </div>
<div class="line">    MyUKF myukf;</div>
<div class="line"> </div>
<div class="line">    <span class="keywordtype">double</span> r = 0.1;</div>
<div class="line">    <span class="keywordtype">double</span> q = 0.1;</div>
<div class="line">    </div>
<div class="line">    Q = eye(3,3)*q*q;</div>
<div class="line">    R &lt;&lt; r*r &lt;&lt; endr;</div>
<div class="line"> </div>
<div class="line">    colvec x0(3);</div>
<div class="line">    x0 &lt;&lt; 0 &lt;&lt; 0 &lt;&lt; 1;</div>
<div class="line"> </div>
<div class="line">    mat P0 = eye&lt;mat&gt;(3, 3);</div>
<div class="line">    P0 =  P0;</div>
<div class="line"> </div>
<div class="line">    colvec u;</div>
<div class="line"> </div>
<div class="line">    <span class="comment">// No inputs</span></div>
<div class="line">    u = u.zeros();</div>
<div class="line"> </div>
<div class="line">    myukf.InitSystem(3, 1, Q, R);</div>
<div class="line">    myukf.InitSystemState(x0);</div>
<div class="line">    myukf.InitSystemStateCovariance(P0);</div>
<div class="line"> </div>
<div class="line">    <span class="keywordflow">for</span> (<span class="keywordtype">int</span> k = 0; k &lt; 20; k++) {</div>
<div class="line">        myukf.UKalmanf(u);</div>
<div class="line"> </div>
<div class="line">        colvec *x = myukf.GetCurrentState();</div>
<div class="line">        colvec *x_m = myukf.GetCurrentEstimatedState();</div>
<div class="line">        colvec *z = myukf.GetCurrentOutput();</div>
<div class="line">        colvec *z_m = myukf.GetCurrentEstimatedOutput();</div>
<div class="line">    }</div>
<div class="line"> </div>
<div class="line">    <span class="keywordflow">return</span> 0;</div>
<div class="line">}</div>
<div class="ttc" id="aclass_u_k_f_html"><div class="ttname"><a href="class_u_k_f.html">UKF</a></div><div class="ttdoc">Implemetation of the Unscented Kalman filter. This class needs to be derived.</div><div class="ttdef"><b>Definition:</b> ukf.h:42</div></div>
<div class="ttc" id="aclass_u_k_f_html_a3121b2c76798ca333cd2b292daa1a32c"><div class="ttname"><a href="class_u_k_f.html#a3121b2c76798ca333cd2b292daa1a32c">UKF::f</a></div><div class="ttdeci">virtual colvec f(const colvec &amp;x, const colvec &amp;u)</div><div class="ttdoc">Define model of your system.</div><div class="ttdef"><b>Definition:</b> ukf.cpp:57</div></div>
<div class="ttc" id="aclass_u_k_f_html_ae0be58f48525b8a77bfbfe0091615519"><div class="ttname"><a href="class_u_k_f.html#ae0be58f48525b8a77bfbfe0091615519">UKF::h</a></div><div class="ttdeci">virtual colvec h(const colvec &amp;x)</div><div class="ttdoc">Define the output model of your system.</div><div class="ttdef"><b>Definition:</b> ukf.cpp:64</div></div>
</div><!-- fragment --><p> <br  />
<br  />
These are the plots that show comparisons between measurements and estimations of the three states: \(x_1\), \(x_2\) and \(x_3\). <br  />
</p><div class="image">
<img src="ex10.png" alt=""/>
</div>
<h1><a class="anchor" id="practical-application"></a>
Practical application: Kalman filter for noisy measurements</h1>
<p>The examples we have so far are theoretical. Very often, what we would like to do is to reduce noise from pre-acquired measurement data. There are several reasons why we want to use Kalman filter. For example, noise has a vast spectrum. Thus, using a frequency-based filter hurts the data. <br  />
<br  />
Principally, there are two scenarios of using the Kalman filtering techniquie here. The first scenario is by first simulating the system, as shown in the figure below.</p>
<p>In this scenario, we only need to supply \(u_k\) to the Kalman filter function. The Kalman procedure will give us four outputs as a result: \(x_k\), \(z_k\), \(\hat{x}_k\), and \(\hat{z}_k\). \(x_k\) and \(z_k\) are called the true states and the true outputs, respectvely. They are noisy. \(\hat{x}_k\), and \(\hat{z}_k\) are called the estimated states and the estimated outputs, respectively. They are filtered, look smoother and closer to the noiseless true states. <br  />
<br  />
The function prototype for the scenario above can be written as: <br  />
[ \(x_k\), \(z_k\), \(\hat{x}_k\), \(\hat{z}_k\)] = function kalmanf( \(u_k\)) <br  />
</p><div class="image">
<img src="Kalman_concept1.jpg" alt=""/>
</div>
<p> <br  />
<br  />
The second scenario is used when the measurements are available. Thus, simulating the system becomes unnecessary. In such a scenario, we need to supply both \(z_k\) and \(u_k\) to the kalman filter function. The Kalman filter will give us 2 outputs: \(\hat{x}_k\) (the estimated system sates) and \(\hat{z}_k\) (the estimated system outputs). <br  />
<br  />
The function prototype for this scenario can be written as: <br  />
[ \(\hat{x}_k\), \(\hat{z}_k\)] = function kalmanf( \(z_k\), \(u_k\)) <br  />
</p><div class="image">
<img src="Kalman_concept2.jpg" alt=""/>
</div>
<p> <br  />
The second scenario is useful for smoothing noisy measurment data. However, both scenarios are availabe in this library. <br  />
 </p>
<h2><a class="anchor" id="practical_example"></a>
Example</h2>
<p>As one example, let us see the problem provided in <a class="el" href="main1_8cpp.html" title="Example for the Kalman filter.">main1.cpp</a>. This time, however, we assume we have already had several noisy data points from measurements. Therfore, we will not need the Kalman procedure to simulate the system. <br  />
The system model can be formulated into: <br  />
 </p><p class="formulaDsp">
\[x_k = x_{k-1} + v_{k-1}\]
</p>
 <p class="formulaDsp">
\[z_k = x_k + w_ḳ\]
</p>
<p> <br  />
We then generate random number as the prerecorded noisy data (<b>z_measurement</b>), as follows. <br  />
</p><div class="fragment"><div class="line"><span class="comment">// Assume we have a noisy signal that is acquired from a measurement tool</span></div>
<div class="line"><span class="keywordtype">double</span> w = 2; <span class="comment">// Stdev of the noise, in reality we don&#39;t know this</span></div>
<div class="line">colvec z_measurement(1);</div>
<div class="line">z_measurement.randn(1);</div>
<div class="line">z_measurement = z_measurement * w + 12.0;</div>
</div><!-- fragment --><p>After than, <b>z_measurement</b> is sent to the Kalman filter function, as the following: <br  />
<br  />
</p><div class="fragment"><div class="line"><span class="comment">// Put the z_measurment to the Kalman filter</span></div>
<div class="line">kalman.<a class="code" href="class_k_f.html#acc63e676e77b8a6b1ccbb88dec545d99">Kalmanf</a>(z_measurement, u);</div>
<div class="line"> </div>
<div class="line">colvec *z_m = kalman.<a class="code" href="class_k_f.html#a5766b144d9d9d4ff395ed546e6071b76">GetCurrentEstimatedOutput</a>();</div>
<div class="line">colvec *x_m = kalman.<a class="code" href="class_k_f.html#abcfd6d77410e71d6df11f356ac49e6b7">GetCurrentEstimatedState</a>();</div>
<div class="ttc" id="aclass_k_f_html_a5766b144d9d9d4ff395ed546e6071b76"><div class="ttname"><a href="class_k_f.html#a5766b144d9d9d4ff395ed546e6071b76">KF::GetCurrentEstimatedOutput</a></div><div class="ttdeci">colvec * GetCurrentEstimatedOutput()</div><div class="ttdoc">Get current estimated output. This is the filtered measurements, with less noise.</div><div class="ttdef"><b>Definition:</b> kf.cpp:125</div></div>
<div class="ttc" id="aclass_k_f_html_abcfd6d77410e71d6df11f356ac49e6b7"><div class="ttname"><a href="class_k_f.html#abcfd6d77410e71d6df11f356ac49e6b7">KF::GetCurrentEstimatedState</a></div><div class="ttdeci">colvec * GetCurrentEstimatedState()</div><div class="ttdoc">Get current estimated state.</div><div class="ttdef"><b>Definition:</b> kf.cpp:120</div></div>
</div><!-- fragment --><p> <br  />
<br  />
The <b>kalman.Kalmanf(z_measurement, u)</b> function above has different parameters with what we can find in <a class="el" href="main1_8cpp.html" title="Example for the Kalman filter.">main1.cpp</a>. Since <b>z_measurement</b> is available, we then send it as a parameter for the <b>kalmanf</b> function. The processes above are done in several iterations according to the number of the availabe data points. The final codes will look like as the following: <br  />
<br  />
</p><div class="fragment"><div class="line"><span class="keywordtype">int</span> <a class="code" href="main1_8cpp.html#a3c04138a5bfe5d72780bb7e82a18e627">main</a>(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span>** argv)</div>
<div class="line">{      </div>
<div class="line">  <span class="comment">// Log the result into a tab delimitted file, later we can open </span></div>
<div class="line">  <span class="comment">// it with Matlab. Use: plot_data7.m to plot the results.</span></div>
<div class="line">  </div>
<div class="line">  ofstream log_file;</div>
<div class="line">  log_file.open(<span class="stringliteral">&quot;log_file7.txt&quot;</span>);</div>
<div class="line">  </div>
<div class="line">  <span class="comment">// Define the system and initialize the Kalman filter</span></div>
<div class="line">   </div>
<div class="line">  mat A(1,1), B(1,1), H(1,1), Q(1,1), R(1,1);</div>
<div class="line">  colvec u(1);</div>
<div class="line">  colvec x0(1);  </div>
<div class="line">    </div>
<div class="line">  A &lt;&lt; 1;</div>
<div class="line">  B &lt;&lt; 1;</div>
<div class="line">  Q &lt;&lt; 0.01; <span class="comment">// Heuristic tuning parameter</span></div>
<div class="line">  H &lt;&lt; 1;</div>
<div class="line">  R &lt;&lt; 1;    <span class="comment">// Heuristic tuning parameter</span></div>
<div class="line">  u &lt;&lt; 0;</div>
<div class="line">    </div>
<div class="line">  <a class="code" href="class_k_f.html">KF</a> kalman;</div>
<div class="line">  kalman.<a class="code" href="class_k_f.html#a890367dd9a8888a2ea8bda296b6128dd">InitSystem</a>(A, B, H, Q, R);</div>
<div class="line">    </div>
<div class="line">  <span class="keywordtype">int</span> N = 500;</div>
<div class="line">  </div>
<div class="line">  <span class="keywordflow">for</span> (<span class="keywordtype">int</span> k = 0; k &lt; N; k++) {</div>
<div class="line">        </div>
<div class="line">    <span class="comment">// Assume we have a noisy signal that is acquired from a measurement tool</span></div>
<div class="line">    <span class="keywordtype">double</span> w = 2; <span class="comment">// Stdev of the noise, in reality we don&#39;t know this</span></div>
<div class="line">    colvec z_measurement(1);</div>
<div class="line">    z_measurement.randn(1);</div>
<div class="line">    z_measurement = z_measurement * w + 12.0;</div>
<div class="line">      </div>
<div class="line">    <span class="comment">// Put the z_measurment to the Kalman filter</span></div>
<div class="line">    kalman.<a class="code" href="class_k_f.html#acc63e676e77b8a6b1ccbb88dec545d99">Kalmanf</a>(z_measurement, u);</div>
<div class="line">      </div>
<div class="line">    colvec *z_m = kalman.<a class="code" href="class_k_f.html#a5766b144d9d9d4ff395ed546e6071b76">GetCurrentEstimatedOutput</a>();</div>
<div class="line">    colvec *x_m = kalman.<a class="code" href="class_k_f.html#abcfd6d77410e71d6df11f356ac49e6b7">GetCurrentEstimatedState</a>();</div>
<div class="line">     </div>
<div class="line">    log_file &lt;&lt; k &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; z_measurement.at(0,0) &lt;&lt; <span class="charliteral">&#39;\t&#39;</span> &lt;&lt; z_m-&gt;at(0,0)  </div>
<div class="line">             &lt;&lt; <span class="charliteral">&#39;\n&#39;</span>; </div>
<div class="line">  }</div>
<div class="line">  <span class="keywordflow">return</span> 0;</div>
<div class="line">}</div>
</div><!-- fragment --><p> <br  />
<br  />
The values that are given to <b>Q</b> and <b>R</b> are defined <b>heuristically</b> since we do not know the actual variances of the noise of the data. <br  />
</p><div class="image">
<img src="ex7.png" alt=""/>
</div>
 </div></div><!-- PageDoc -->
</div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="footer">Generated on Thu Apr 1 2021 04:15:37 for kalman-cpp by <a href="https://www.doxygen.org/index.html"><img class="footer" src="doxygen.svg" width="104" height="31" alt="doxygen"/></a> 1.9.2 </li>
  </ul>
</div>
</body>
</html>
